using System;
using System.Collections.Generic;
using Opsive.UltimateCharacterController.Utility;
using RootMotion.Dynamics;
using Unity.Mathematics;
using UnityEngine;
using Yoozoo.Gameplay.Liberty.AI;
using Yoozoo.Mars.Got;

namespace Yoozoo.Gameplay.Liberty.PlayerController
{
    [RequireComponent(typeof(Animator))]
    public class InputRootMotion : MonoBehaviour
    {
        [Tooltip("The rate at which the character can rotate. Only used by non-root motion characters.")]
        public float m_MotorRotationSpeed = 0.15f;
        private PuppetMaster _puppetMaster;
        private Animator _animator;
        private Rigidbody _rigidbody;
        private CapsuleCollider _capsuleCollider;
        private AIEntity playerEntity;
        public LayerMask groundCastMask;
        public float groundCastDistance = 0.2f;
        public float gravity = -9.8f;
        [SerializeField]
        private bool active;
        public bool Active
        {
            get => active;
            set => active = value;
        }
        [Tooltip("The maximum number of colliders that the character can detect.")]
        [SerializeField] protected int m_MaxCollisionCount = 100;
        [Tooltip("The maximum number of frames that the soft force can be distributed by.")]
        [SerializeField] protected int m_MaxSoftForceFrames = 100;
        [Tooltip("The maximum number of collision checks that should be performed when rotating.")]
        [SerializeField] protected int m_MaxRotationCollisionChecks = 10;
        [Tooltip("The maximum object slope angle that the character can traverse (in degrees).")]
        [SerializeField] protected float m_SlopeLimit = 50f;
        [Tooltip("角色的运动力加速率。仅用于非根动画的角色。")]
        [SerializeField] protected Vector3 m_MotorAcceleration = new Vector3(3.2f, 0, 3.2f);
        [Tooltip("角色的运动力减速率。仅用于非根动画的角色。")]
        [Header("角色的运动力减速率")]
        [SerializeField] protected float m_MotorDamping = 5.4f;
        [Tooltip("在向后移动时的力的系数。")]
        [SerializeField] protected float m_MotorBackwardsMultiplier = 0.7f;
        [Tooltip("A (0-1) value specifying the amount of influence the previous acceleration direction has on the current velocity.")]
        [Range(0, 1)] [SerializeField] protected float m_PreviousAccelerationInfluence = 1;
        [Space(10)] [Header("人物碰撞检测层级")]      
        [Tooltip("The up direction of the character.")]
        [SerializeField] protected Vector3 m_Up = Vector3.up;
        [Tooltip("Should gravity be applied?")]
        [SerializeField] protected bool m_UseGravity = true;
        [Tooltip("The amount of gravity that should be accumulated each move.")]
        [SerializeField] protected float m_GravityAmount = 0.2f;
        [Tooltip("The direction of the gravity.")]
        [SerializeField] protected Vector3 m_GravityDirection = Vector3.down;
        [Tooltip("Should the character check for collisions even when not moving?")]
        [SerializeField] protected bool m_ContinuousCollisionDetection = true;
        [Tooltip("The maximum number of penetration checks against a single collider.")]
        [SerializeField] protected int m_MaxPenetrationChecks = 5;
        [UnityEngine.Serialization.FormerlySerializedAs("m_GroundDetection")] // 3.0.9.
        [SerializeField] protected bool m_DetectGround = true;
        [Tooltip("Can the character detect vertical collisions?")]
        [SerializeField] protected bool m_DetectVerticalCollisions = true;
        [Tooltip("The maximum height that the character can step on top of.")]
        [SerializeField] protected float m_MaxStepHeight = 0.35f;
        [Tooltip("A multiplier to apply to the physic material bounce value when colliding with the wall. Allows for the character to bounce off the wall.")]
        [SerializeField] protected float m_WallBounceModifier = 2;
        [Tooltip("A curve specifying the amount to move when gliding along a wall. The x variable represents the dot product between the character look direction and wall normal. " +
                 "An x value of 0 means the character is looking directly at the wall. An x value of 1 indicates the character is looking parallel to the wall.")]
        [SerializeField] protected AnimationCurve m_WallGlideCurve = new AnimationCurve(new Keyframe[] { new Keyframe(0, 0), new Keyframe(0.1f, 0.5f, 0, 0), new Keyframe(1, 0.5f) });
        [Tooltip("A multiplier to apply to the physic material friction value when colliding with the wall.")]
        [SerializeField] protected float m_WallFrictionModifier = 1f;
        [Tooltip("The maximum number of collision checks that should be performed when moving.")]
        [SerializeField] protected int m_MaxMovementCollisionChecks = 5;
        [Tooltip("Should the character stick to the ground?")]
        [SerializeField] protected bool m_StickToGround = true;
        [Tooltip("The distance that should be used when the character is sticking to the ground.")]
        [SerializeField] protected float m_StickToGroundDistance = 0.3f;
        [Tooltip("Specifies the width of the characters skin, use for ground detection.")]
        [SerializeField] protected float m_SkinWidth = 0.08f;
        [Tooltip("A multiplier to apply to the physic material bounce value when colliding with the ground. Allows for the character to bounce off of the ground.")]
        [SerializeField] protected float m_GroundBounceModifier = 1;
        [Tooltip("A multiplier to apply to the physic material friction value when colliding with the ground.")]
        [SerializeField] protected float m_GroundFrictionModifier = 10f;
        [Tooltip("Should the motor force be adjusted while on a slope?")]
        [SerializeField] protected bool m_AdjustMotorForceOnSlope = true;
        [Tooltip("If adjusting the motor force on a slope, the force multiplier when on an upward slope.")]
        [SerializeField] protected float m_MotorSlopeForceUp = 1f;
        [Tooltip("If adjusting the motor force on a slope, the force multiplier when on a downward slope.")]
        [SerializeField] protected float m_MotorSlopeForceDown = 1.25f;
        [Tooltip("The rate at which the character's external force decelerates.")]
        [SerializeField] protected float m_ExternalForceDamping = 0.1f;
        [SerializeField]public LayerMask playerMask;
        public bool UsingGravity
        {
            get { return m_UseGravity; }
        }
        [SerializeField]
        private float verticalVelocity = 0f;
        public bool checkGround = true;
        private float _checkGroundTime = 0;
        protected Collider[] m_Colliders;
        private Collider[] m_IgnoredColliders;
        private GameObject[] m_ColliderGameObjects;
        private GameObject[] m_IgnoredColliderGameObjects;
        protected int m_ColliderCount = 0;
        private RaycastHit[] m_CombinedCastResults;
        protected RaycastHit[] m_CastResults;
        protected RaycastHitComparer m_CastHitComparer = new RaycastHitComparer();
        private int m_ColliderIndex;
        private Collider[] m_OverlapCastResults;
        private Vector3[] m_SoftForceFrames;
        private float m_GravityAccumulation;
        private bool m_ForceGroundCollisionDetection;
        private bool m_ForceVerticalCollisionDetection;
        // Padding value used to prevent the character's collider from overlapping the environment collider. Overlapped colliders don't work well with ray casts.
        private const float c_ColliderSpacing = 0.01f;
        private bool m_AllowGroundCollisionDetection = true;
        private bool m_AllowVerticalCollisionDetection = true;
        private bool m_SlopedGround;
        private float m_Height;
        private bool m_ForceUngrounded = false;
        private float m_Radius = float.MaxValue;
        private RaycastHit m_EmptyRaycastHit = new RaycastHit();
        private Vector3 m_Velocity;
        private Dictionary<RaycastHit, int> m_ColliderIndexMap;
        protected Vector3 m_Position;
        protected Quaternion m_Rotation;
        
        private Vector3 LocalDesiredMovement
        {
            get { return this.InverseTransformDirection(m_DesiredMovement); }
            set {
                if (Math.Abs(value.y) > LocalDesiredMovement.y && m_Grounded) { m_ForceUngrounded = true; }
                m_DesiredMovement = this.TransformDirection(value);
            }
        }
        public Vector3 TargetPosition { get { return m_Position + m_DesiredMovement; } }
        public Quaternion TargetRotation { get { return m_Rotation * m_DesiredRotation; } }
        public float Radius => m_Radius;
        public bool StickingToGround => m_StickToGround;
        public float ColliderSpacing => c_ColliderSpacing;
        public bool AllowVerticalCollisionDetection { set { m_AllowVerticalCollisionDetection = value; } }
        public bool AllowGroundCollisionDetection { set { m_AllowGroundCollisionDetection = value; } }
        public bool ForceVerticalCollisionDetection { set { m_ForceVerticalCollisionDetection = value; } }
        public bool ForceGroundCollisionDetection { set { m_ForceGroundCollisionDetection = value; } }
        public bool UsingGroundCollisionDetection { get { return (m_DetectGround || m_ForceGroundCollisionDetection) && m_AllowGroundCollisionDetection; } }
        public bool UsingVerticalCollisionDetection { get { return (m_DetectVerticalCollisions || m_ForceVerticalCollisionDetection) && m_AllowVerticalCollisionDetection; } }
        
        // Motion
        [SerializeField]
        private Vector3 m_MotionDirection = Vector3.zero;
        [SerializeField] 
        private float m_MotionSpeed = 0;
        
        private void Awake()
        {
            _animator = GetComponent<Animator>();
            _rigidbody = GetComponent<Rigidbody>();
            _capsuleCollider = GetComponent<CapsuleCollider>();
            if (transform.parent != null)
            {
                Transform puppetmasterTrans = transform.parent.Find("PuppetMaster");
                if (puppetmasterTrans != null)
                {
                    _puppetMaster = puppetmasterTrans.GetComponent<PuppetMaster>();
                }
            }
            _animator.updateMode = AnimatorUpdateMode.Normal;
            _animator.cullingMode = AnimatorCullingMode.AlwaysAnimate;
            _rigidbody.collisionDetectionMode = CollisionDetectionMode.ContinuousSpeculative;
            // Initialize the colliders.
            var colliders = GetComponentsInChildren<Collider>(true);
            m_Colliders = new Collider[colliders.Length];
            m_IgnoredColliders = new Collider[colliders.Length];
            for (int i = 0; i < colliders.Length; i++)
            {
                m_Colliders[m_ColliderCount] = colliders[i];
                m_ColliderCount++;
                if (!m_CheckRotationCollision) {
                    m_CheckRotationCollision = CanCauseRotationCollision(colliders[i]);
                }
                
                // Determine the mim radius of the character.
                float radius;
                if (colliders[i] is CapsuleCollider) {
                    radius = (colliders[i] as CapsuleCollider).radius;
                } else if (colliders[i] is SphereCollider) {
                    radius = (colliders[i] as SphereCollider).radius;
                } else { // BoxCollider.
                    radius = (colliders[i] as BoxCollider).size.x / 2; // Not a true radius, but serves the purpose.
                }
                if (radius < m_Radius) {
                    m_Radius = radius;
                }
                
                // Determine the max height of the character based on the collider.
                var height = MathUtils.LocalColliderHeight(transform, colliders[i]);
                if (height > m_Height) {
                    m_Height = height;
                }
            }
            // Physics initialization.
            m_CastResults = new RaycastHit[m_MaxCollisionCount];
            m_OverlapCastResults = new Collider[m_MaxCollisionCount];
            m_SoftForceFrames = new Vector3[m_MaxSoftForceFrames];
            // If there are multiple colliders then save a mapping between the raycast hit and the collider index.
            if (m_Colliders.Length > 1) {
                m_ColliderIndexMap = new Dictionary<RaycastHit, int>(new RaycastHitEqualityComparer());
                m_CombinedCastResults = new RaycastHit[m_CastResults.Length * m_Colliders.Length];
            }
            
            m_Up = m_Rotation * Vector3.up;
            m_PrevMotorRotation = m_Rotation;
        }
        
        public bool isVehicleBarge;
        public bool isVehicleScratch;

        public bool isOutPlayerControl;
        public Vector3 bargeDir;
        
        public AnimationCurve leapCurve;
        public float vehicleBargeTime;
        public float vehicleBargeSpeed = 1;
        public float vehicleBargeHeight = 2;

        public float vehicleBargeDistance = 1.5f;
        private Vector3 m_ExternalForce;

        public void OnSet2InputCamera()
        {
            playerEntity.ResetFixedPosition();
        }
        private void OnEnable()
        {
            if (_rigidbody)
            {
                _rigidbody.useGravity = false;
            }
            _checkGroundTime = 0;
            playerEntity?.ResetFixedPosition();
        }
        
        private void OnDisable()
        {
            _checkGroundTime = 0;
        }

        private void FixedUpdate()
        {
            if (_rigidbody == null)
            {
                _rigidbody = GetComponent<Rigidbody>();
            }
            
            if (!Active) return;
            if (!_rigidbody) return;
            // RestoreFixedLocation();
            AIData aiData = LibertyAIUtils.PlayerController.playerEntity.AIEntity.data;
            if (aiData.hp <= 0)
            {
                return;
            }
            _checkGroundTime += Time.fixedDeltaTime;
            Vector3 velocity = Vector3.zero;

            // if (_animator.updateMode == AnimatorUpdateMode.Normal)
            // {
            //      // velocity = _animator.deltaPosition / Time.deltaTime;
            //      velocity = aiData.moveDirection * aiData.moveSpeed;
            // }
            // else if(_animator.updateMode == AnimatorUpdateMode.AnimatePhysics)
            {
                if (isVehicleBarge)
                {
                    _rigidbody.useGravity = false;
                    vehicleBargeTime += Time.fixedDeltaTime;
                    Vector3 deltaPosition = _animator.deltaPosition;
                    
                    deltaPosition = Vector3.Dot(deltaPosition, bargeDir) * bargeDir * 1.414f * vehicleBargeDistance;
                    float lastH = _rigidbody.transform.position.y;
                    float h = leapCurve.Evaluate(vehicleBargeTime * vehicleBargeSpeed) * vehicleBargeHeight;
                    
                    deltaPosition.y = h - lastH;
                    
                    velocity = deltaPosition / Time.fixedDeltaTime;
                }
                else if (isVehicleScratch)
                {
                    _rigidbody.useGravity = false;
                    Vector3 deltaPosition = _animator.deltaPosition;
                    deltaPosition = Vector3.Dot(deltaPosition, bargeDir) * bargeDir;
                    velocity = deltaPosition / Time.fixedDeltaTime;
                }
                else
                {
                    vehicleBargeTime = 0;

                    // 根据动画状态区分是否使用RootMotion
                    if (aiData.animationStateHasRootMotion)
                    {
                        velocity = _animator.deltaPosition / Time.fixedDeltaTime;
                        velocity += aiData.moveDirection * aiData.moveSpeed;
                    }
                    else
                    {
                        velocity = aiData.moveDirection * aiData.moveSpeed;
                    }

                    if (checkGround && _checkGroundTime > 2.5f && !IsGround())
                    {
                        if (!_rigidbody.useGravity)
                        {
                            _rigidbody.useGravity = true;
                        }
                        
                        verticalVelocity = -5f;
                        verticalVelocity += gravity * Time.fixedDeltaTime;
                        velocity.y = verticalVelocity;
                    }
                    else
                    {
                        verticalVelocity = 0f;
                    }
                }
            }

            m_MotionDirection = velocity.normalized;
            m_MotionSpeed = velocity.magnitude;
            _rigidbody.isKinematic = true;
            // Fix: Unity升级到2022之后，当Physics.simulationMode为PhysicsSimulationMode.FixedUpdate模式时，当前这个子节点会莫名其妙的被移动，造成位移错误；
            // 当Physics.simulationMode为PhysicsSimulationMode.Update模式时，当前这个子节点不会被移动，很奇怪；
            transform.SetLocalPositionAndRotation(Vector3.zero, Quaternion.identity);
            // MoveCharacters(true, -1);
            MoveCharacters(false, -1);
        }
        public virtual void ResetRotationPosition()
        {
            m_Position = playerEntity.data.CurrentLerpPositin;
            // m_Rotation = m_PrevMotorRotation = Quaternion.Euler(playerEntity.data.CurrentRotation);
            m_DesiredRotation = Quaternion.identity;
            m_MotorThrottle = m_DesiredMovement = m_ExternalForce = Vector3.zero;
            m_GravityAccumulation = 0;
            m_Up = m_Rotation * Vector3.up;
            m_SlopedGround = false;
        }
    
        public void MoveCharacters(bool preMove, float interpAmount)
        {
            if (interpAmount == -1)
            {
                Move(preMove);
            }
            else
            {
                SmoothMove(preMove, interpAmount);
            }
        }
        /// <summary>
        /// The object is moved within FixedUpdate while the camera is moved within Update. This would normally cause jitters but a separate smooth variable
        /// ensures the object stays in synchronize with the Update loop.
        /// </summary>
        /// <param name="preMove">Should the characters be premoved?</param>
        /// <param name="interpAmount">The amount to interpolate between the smooth and fixed position.</param>
        public void SmoothMove(bool preMove, float interpAmount)
        {
            // transform.position = Vector3.Lerp(m_SmoothPosition, m_FixedPosition, interpAmount);
            // // transform.SetPositionAndRotation(Vector3.Lerp(m_SmoothPosition, m_FixedPosition, interpAmount), Quaternion.Slerp(m_SmoothRotation, m_FixedRotation, interpAmount));
            // if (_rigidbody != null) {
            //     _rigidbody.position = transform.position;
            //     // _rigidbody.rotation = transform.rotation;
            // }
        }

        /// <summary>
        /// Moves the character.
        /// </summary>
        /// <param name="preMove">Should the character be premoved?</param>
        public void Move(bool preMove)
        {
            // if (preMove) {
            //     playerEntity.RestoreFixedLocation();
            //     return;
            // }
            // Reset the previous values.
            m_DesiredMovement = Vector3.zero;
            m_DesiredRotation = Quaternion.identity;
            Move(m_MotionDirection, m_MotionSpeed);
            AssignFixedLocation();
        }

        private void AssignFixedLocation()
        {
            playerEntity?.AssignFixedLocation(TargetPosition);
        }

        protected Vector2 m_InputVector;
        protected Vector3 m_DeltaRotation;
        private Vector3 m_DesiredMovement;
        private Quaternion m_DesiredRotation = Quaternion.identity;
        private Vector3 m_LastDesiredMovement;
        private Quaternion m_LastDesiredRotation = Quaternion.identity;
        private bool m_Grounded;
        private RaycastHit m_GroundedRaycastHit;
        private Collider m_CharacterGroundedCollider;
        private Vector3 m_MotorThrottle;
        private float m_SlopeFactor = 1;
        private Quaternion m_PrevMotorRotation;
        private void Move(Vector3 aiDataMoveDirection, float aiDataMoveSpeed)
        {
            // Assign the inputs.
            m_InputVector.x = aiDataMoveDirection.x * aiDataMoveSpeed;
            m_InputVector.y = aiDataMoveDirection.z * aiDataMoveSpeed;
            // Reset the previous values.
            m_DesiredMovement = Vector3.zero;
            m_DesiredRotation = Quaternion.identity;
            UpdateCharacter();
        }

        private void UpdateCharacter()
        {
            // Update the rotation before the position so the forces will be applied in the correct direction.
            UpdateRotation();

            // Set the new rotation.
            ApplyRotation();

            // Update the position values based on the input.
            UpdatePosition();

            // The movement values have been determined. Update the DesiredMovement before the collision and ground checks.
            UpdateDesiredMovement();

            // Update the external forces after the movement has been applied.
            // This should be done after the movement is applied so the full force value has been applied within UpdatePosition.
            UpdateExternalForces();

            // Check for collisions.
            DetectCollisions();

            // Is the character still on the ground?
            DetectGroundCollision();

            // Set the new position.
            ApplyPosition();
        }
        
        /// <summary>
        /// Check for a horizontal collision.
        /// </summary>
        private void DetectCollisions()
        {
            if (m_DesiredMovement.sqrMagnitude == 0) {
                // There may be other objects that intersect with the character even if the character isn't moving.
                if (m_ContinuousCollisionDetection) {
                    for (int i = 0; i < m_ColliderCount; ++i) {
                        if (!m_Colliders[i].gameObject.activeInHierarchy) {
                            continue;
                        }

                        if (OverlapColliders(m_Colliders[i], m_Position + m_Up * c_ColliderSpacing, m_Rotation * m_DesiredRotation, -c_ColliderSpacing * 2) > 0) {
                            if (ResolvePenetrations(m_Colliders[i], Vector3.zero, out var offset)) {
                                var localOffset = this.InverseTransformDirection(offset);
                                localOffset.y = 0;
                                m_DesiredMovement += this.TransformDirection(offset);
                            }
                        }

                    }
                }
                if (m_DesiredMovement.sqrMagnitude == 0) {
                    return;
                }
            }

            var targetPosition = m_Position + m_DesiredMovement;
            var lastPosition = m_Position;// + m_MovingPlatformMovement;
            var iterations = 0;
            bool settled;
            var slideDirection = Vector3.zero;
            var slide = false;
            do {
                settled = true;
                var targetMovement = targetPosition - lastPosition;
                var movementMagnitude = targetMovement.magnitude;
                var normalizedTargetMovement = targetMovement.normalized;
                var hitCount = CombinedCast(lastPosition, normalizedTargetMovement, Mathf.Max(movementMagnitude, c_ColliderSpacing));
                if (hitCount > 0) {
                    for (int i = 0; i < hitCount; ++i) {
                        // Determine which collider caused the intersection.
                        var closestRaycastHit = QuickSelect.SmallestK(m_CombinedCastResults, hitCount, i, m_CastHitComparer);
                        var activeCollider = m_ColliderCount > 1 ? m_Colliders[m_ColliderIndexMap[closestRaycastHit]] : m_Colliders[m_ColliderIndex];

                        if (closestRaycastHit.distance == 0) {
                            if (ResolvePenetrations(activeCollider, targetPosition - m_Position, out var offset)) {
                                // Stop moving if two characters are running directly into each other.
                                if (closestRaycastHit.rigidbody != null && closestRaycastHit.collider.gameObject.GetCachedParentComponent<InputRootMotion>() != null) {
                                    if (Vector3.Dot(offset.normalized, normalizedTargetMovement) < -0.99f) {
                                        var localLastPosition = this.InverseTransformPoint(lastPosition);
                                        localLastPosition.y = this.InverseTransformPoint(targetPosition).y;
                                        targetPosition = this.TransformPoint(localLastPosition);
                                        break;
                                    }
                                }

                                // Ground check will handle vertical collisions.
                                if (!UsingGroundCollisionDetection || !UsingVerticalCollisionDetection || Mathf.Abs(Vector3.Dot(m_Up, this.InverseTransformDirection(closestRaycastHit.normal))) < 0.9995f) {
                                    targetPosition += offset * 0.1f;
                                    settled = false;
                                }
                            } else {
                                // The vertical direction should not be affected.
                                var localLastPosition = this.InverseTransformPoint(lastPosition);
                                localLastPosition.y = this.InverseTransformPoint(targetPosition).y;
                                targetPosition = this.TransformPoint(localLastPosition);
                            }
                            break;
                        }

                        // If the object is directly beneath the character then DetectGround will handle it.
                        if (Vector3.Dot(closestRaycastHit.normal, m_Up) == 1) {
                            continue;
                        }

                        // If the object is a rigidbody then the character should try to push it.
                        PushRigidbody(closestRaycastHit.rigidbody, closestRaycastHit.point, closestRaycastHit.normal, movementMagnitude);

                        // A CapsuleCast/SphereCast normal isn't always the true normal: http://answers.unity3d.com/questions/50825/raycasthitnormal-what-does-it-really-return.html.
                        // Cast a regular raycast in order to determine the true normal.
                        var ray = new Ray(closestRaycastHit.point + normalizedTargetMovement * c_ColliderSpacing + m_Up * (m_MaxStepHeight + c_ColliderSpacing), -m_Up);
                        if (!Physics.Raycast(ray, out var slopeRaycastHit, m_MaxStepHeight + c_ColliderSpacing * 2, playerMask, QueryTriggerInteraction.Ignore)) {
                            slopeRaycastHit = closestRaycastHit;
                        }

                        // Only check for steps if the hit point is lower than the max step height.
                        var slopeAngle = Vector3.Angle(m_Up, slopeRaycastHit.normal);
                        var directionalSlope = false;
                        var groundPoint = MathUtils.InverseTransformPoint(m_Position, m_Rotation * m_DesiredRotation,
                            closestRaycastHit.point);//- m_MovingPlatformMovement);
                        if (groundPoint.y > 0 && (groundPoint.y <= m_MaxStepHeight || groundPoint.y < movementMagnitude)) {
                            if (slopeAngle >= 89.999f) {
                                // Cast a ray directly in front of the character. If it doesn't hit an object then the object is shorter than the step height and should be stepped on.
                                // Continue out of the loop to prevent the character from stopping in front of the object.
                                if (!SingleCast(activeCollider, (targetPosition - m_Position).normalized, /*m_MovingPlatformMovement + */
                                        m_Up * (m_MaxStepHeight - c_ColliderSpacing),
                                                                    (m_Radius + movementMagnitude), playerMask)) {
                                    continue;
                                }
                            } else if (slopeAngle <= m_SlopeLimit) {
                                directionalSlope = true;
                                m_SlopedGround = true;
                                continue;
                            }
                        }

                        var minDistance = Mathf.Max(Mathf.Min(movementMagnitude, closestRaycastHit.distance - c_ColliderSpacing), 0);
                        // Stop moving if a ceiling is near the character.
                        if (closestRaycastHit.distance < movementMagnitude && Vector3.Dot(closestRaycastHit.normal, -m_Up) > 0.995) {
                            minDistance = 0;
                        } else if (!directionalSlope) {
                            // The character may bounce away from the object. This bounce is applied to the external force so it'll be checked next frame.
                            if (m_WallBounceModifier > 0) {
                                var bouncinessValue = MathUtils.BouncinessValue(activeCollider.material, closestRaycastHit.collider.material);
                                if (bouncinessValue > 0.0f) {
                                    var magnitude = m_DesiredMovement.magnitude * bouncinessValue * m_WallBounceModifier / Time.deltaTime;
                                    AddForce(Vector3.Reflect(normalizedTargetMovement, closestRaycastHit.normal).normalized * magnitude, 1, false);
                                }
                            }

                            // The movement isn't settled until the character has moved the full potential distance. The character can glide across walls and slopes.
                            float movementDot;
                            var projectedNormal = Vector3.ProjectOnPlane(closestRaycastHit.normal, m_Up);
                            if (closestRaycastHit.distance < movementMagnitude && (movementDot = Vector3.Dot(normalizedTargetMovement, projectedNormal)) > -0.99f) {
                                var cross = Vector3.Cross(slopeAngle <= m_SlopeLimit + c_ColliderSpacing ? closestRaycastHit.normal : projectedNormal, normalizedTargetMovement);
                                var forward = Vector3.Cross(cross, closestRaycastHit.normal).normalized;
                                if (forward.sqrMagnitude > 0) {
                                    // Prevent root motion from causing the character to slide in the wrong direction.
                                    // if (UsingRootMotionPosition && Vector3.Dot(forward, m_RootMotionDeltaPosition.normalized) < 0.01f) {
                                    //     forward = Vector3.Cross(Vector3.Cross(closestRaycastHit.normal, normalizedTargetMovement), closestRaycastHit.normal).normalized;
                                    // }
                                    // The vertical movement should not change unless the character hits a ceiling.
                                    float localVerticalPosition;
                                    if (Vector3.Dot(closestRaycastHit.normal, -m_Up) > 0.01f) {
                                        localVerticalPosition = Mathf.Min(this.InverseTransformPoint(targetPosition).y, closestRaycastHit.distance);
                                    } else {
                                        localVerticalPosition = this.InverseTransformPoint(targetPosition).y;
                                    }

                                    // Prevent the character from jittering between positions by ensuring the character slides in the same direction.
                                    if (slideDirection.sqrMagnitude > 0 && Vector3.Dot(slideDirection, forward) > 0 && this.InverseTransformPoint(closestRaycastHit.point).y > 0) {
                                        var localLastPosition = this.InverseTransformPoint(lastPosition);
                                        targetPosition = this.TransformPoint(new Vector3(localLastPosition.x, localVerticalPosition, localLastPosition.z));
                                        break;
                                    }
                                    settled = false;
                                    slideDirection = forward;
                                    slide = true;

                                    var dynamicFrictionValue = (1 - Mathf.Clamp01(MathUtils.FrictionValue(activeCollider.material, closestRaycastHit.collider.material, true))) * m_WallFrictionModifier;
                                    lastPosition = (lastPosition + normalizedTargetMovement * minDistance);
                                    targetPosition = lastPosition + (movementMagnitude - closestRaycastHit.distance) * m_WallGlideCurve.Evaluate(1 - movementDot) * dynamicFrictionValue * forward;

                                    // Keep the same vertical movement.
                                    var localTargetPosition = this.InverseTransformPoint(targetPosition);
                                    targetPosition = this.TransformPoint(new Vector3(localTargetPosition.x, localVerticalPosition, localTargetPosition.z));

                                    break;
                                }
                            }
                        }

#if UNITY_EDITOR
                        Debug.DrawRay(closestRaycastHit.point, closestRaycastHit.normal, Color.red);
#endif
                        targetPosition = lastPosition + normalizedTargetMovement * minDistance;

                        if (Vector3.Dot(m_ExternalForce.normalized, m_Up) > 0.99f) {
                            // The vertical external force should be cancelled out.
                            var localExternalForce = this.InverseTransformDirection(m_ExternalForce);
                            localExternalForce.y = 0;
                            m_ExternalForce = this.TransformDirection(localExternalForce);
                        }

                        // Others may be interested in the collision.
                        // if (m_OnCollision != null) {
                        //     m_OnCollision(closestRaycastHit);
                        // }

                        break;
                    }
                }
                iterations++;
            } while (!settled && iterations < m_MaxMovementCollisionChecks);

            m_DesiredMovement = targetPosition - m_Position;

            // If the character is sliding along the wall the resulting vector may cause a collision. Ensure the method doesn't end in a collision state.
            if (slide) {
                var targetDesiredMovement = m_DesiredMovement;
                for (int i = 0; i < m_ColliderCount; ++i) {
                    // The collider may not be enabled.
                    if (!m_Colliders[i].gameObject.activeInHierarchy) {
                        continue;
                    }

                    if (ResolvePenetrations(m_Colliders[i], m_DesiredMovement, out var offset)) {
                        m_DesiredMovement += offset;
                    }
                }

                // Rest the desired movement if the target direction is going in the wrong direction.
                if (Vector3.Dot(m_DesiredMovement.normalized, targetDesiredMovement.normalized) < 0.5f) {
                    m_DesiredMovement = targetDesiredMovement;
                }
            }
        }
        /// <summary>
        /// Casts a ray in the specified direction. If the character has multiple colliders added then a ray will be cast from each collider.
        /// A CapsuleCast, SphereCast or BoxCast is used depending on the type of collider that has been added. The result is stored in the m_CombinedCastResults array.
        /// </summary>
        /// <param name="position">The position that the cast should be started from.</param>
        /// <param name="direction">The direction of the cast. This value should be normalized.</param>
        /// <param name="distance">The distance of the cast.</param>
        /// <param name="drawDebugLine">Should the debug lines be drawn?</param>
        /// <returns>The number of objects hit from the cast.</returns>
        private int CombinedCast(Vector3 position, Vector3 direction, float distance
#if UNITY_EDITOR
            , bool drawDebugLine = false
#endif
            )
        {
            if (m_ColliderCount > 1) {
                // Clear the index map to start it off fresh.
                m_ColliderIndexMap.Clear();
            }

            var rigidbodyRotation = m_Rotation;
            var positionOffset = MathUtils.InverseTransformPoint(m_Position, rigidbodyRotation, position);
            var hitCount = 0;
            for (int i = 0; i < m_ColliderCount; ++i) {
                // The collider may not be enabled.
                if (!m_Colliders[i].gameObject.activeInHierarchy) {
                    continue;
                }

                int localHitCount;
                var colliderTransform = m_Colliders[i].transform;
                var castPosition = MathUtils.TransformPoint(colliderTransform.position, rigidbodyRotation, positionOffset);
                // Determine if the collider would intersect any objects.
                if (m_Colliders[i] is CapsuleCollider) {
                    var capsuleCollider = m_Colliders[i] as CapsuleCollider;
                    MathUtils.CapsuleColliderEndCaps(capsuleCollider, castPosition, colliderTransform.rotation, out var capsuleEndCap1, out var capsuleEndCap2);
                    var radius = capsuleCollider.radius * MathUtils.ColliderScaleMultiplier(capsuleCollider) - c_ColliderSpacing;
                    localHitCount = Physics.CapsuleCastNonAlloc(capsuleEndCap1, capsuleEndCap2, radius, direction, m_CastResults, distance, playerMask, QueryTriggerInteraction.Ignore);
#if UNITY_EDITOR
                    if (drawDebugLine) {
                        Debug.DrawLine(capsuleEndCap1, capsuleEndCap2, Color.green);
                        Debug.DrawRay(capsuleEndCap1, direction * Mathf.Min(distance, 100), Color.blue);
                    }
#endif
                } else if (m_Colliders[i] is SphereCollider) {
                    var sphereCollider = m_Colliders[i] as SphereCollider;
                    var radius = sphereCollider.radius * MathUtils.ColliderScaleMultiplier(sphereCollider) - c_ColliderSpacing;
                    localHitCount = Physics.SphereCastNonAlloc(castPosition + colliderTransform.TransformDirection(sphereCollider.center), radius, direction,
                                                                    m_CastResults, distance, playerMask, QueryTriggerInteraction.Ignore);
#if UNITY_EDITOR
                    if (drawDebugLine) {
                        Debug.DrawRay(position + sphereCollider.transform.TransformDirection(sphereCollider.center) + positionOffset, direction * Mathf.Min(distance, 100), Color.blue);
                    }
#endif
                } else { // BoxCollider.
                    var boxCollider = m_Colliders[i] as BoxCollider;
                    var extents = (MathUtils.ColliderScaleMultiplier(boxCollider) - c_ColliderSpacing) * boxCollider.size / 2;
                    localHitCount = Physics.BoxCastNonAlloc(castPosition + colliderTransform.TransformDirection(boxCollider.center), extents, direction,
                                                                    m_CastResults, colliderTransform.rotation, distance, playerMask, QueryTriggerInteraction.Ignore);
#if UNITY_EDITOR
                    if (drawDebugLine) {
                        Debug.DrawRay(position + boxCollider.transform.TransformDirection(boxCollider.center) + positionOffset, direction * Mathf.Min(distance, 100), Color.blue);
                    }
#endif
                }

                if (localHitCount > 0) {
                    // The mapping needs to be saved if there are multiple colliders.
                    if (m_ColliderCount > 1) {
                        int validHitCount = 0;
                        for (int j = 0; j < localHitCount; ++j) {
                            if (m_ColliderIndexMap.ContainsKey(m_CastResults[j])) {
                                continue;
                            }
                            // Ensure the array is large enough.
                            if (hitCount + validHitCount >= m_CombinedCastResults.Length) {
                                Debug.LogWarning("Warning: The maximum number of collisions has been reached. Consider increasing the CharacterLocomotion MaxCollisionCount value.");
                                continue;
                            }

                            m_ColliderIndexMap.Add(m_CastResults[j], i);
                            m_CombinedCastResults[hitCount + validHitCount] = m_CastResults[j];
                            validHitCount += 1;
                        }
                        hitCount += validHitCount;
                    } else {
                        m_CombinedCastResults = m_CastResults;
                        hitCount += localHitCount;
                        m_ColliderIndex = i;
                    }
                }
            }

            return hitCount;
        }
        
        /// <summary>
        /// Pushes the target Rigidbody in the specified direction.
        /// </summary>
        /// <param name="targetRigidbody">The Rigidbody to push.</param>
        /// <param name="point">The point at which to apply the push force.</param>
        /// <param name="normal">The normal of the hit Rigidbody.</param>
        /// <param name="movementMagnitude">The magnitude of the character's movement.</param>
        private void PushRigidbody(Rigidbody targetRigidbody, Vector3 point, Vector3 normal, float movementMagnitude)
        {
            if (targetRigidbody == null || targetRigidbody.isKinematic) {
                return;
            }

            targetRigidbody.AddForceAtPosition((_rigidbody.mass / targetRigidbody.mass) * movementMagnitude * -normal, point);
        }
        /// <summary>
        /// Casts a ray using in the specified direction.  A CapsuleCast or SphereCast is used depending on the type of collider that has been added.
        /// The result is stored in the m_RaycastHit object.
        /// </summary>
        /// <param name="castCollider">The collider which is performing the cast0.84</param>
        /// <param name="direction">The direction to perform the cast.</param>
        /// <param name="offset">Any offset to apply to the cast.</param>
        /// <param name="distance">The distance of the cast.</param>
        /// <param name="layers">The layers to perform the cast on.</param>
        /// <returns>Did the cast hit an object?</returns>
        protected bool SingleCast(Collider castCollider, Vector3 direction, Vector3 offset, float distance, int layers
#if UNITY_EDITOR
            , bool drawDebugLine = false
#endif
        )
        {
            return SingleCast(castCollider, direction, offset, distance, layers, out var raycastHit
#if UNITY_EDITOR
                , drawDebugLine
#endif
            );
        }
        
        /// <summary>
        /// Casts a ray using in the specified direction.  A CapsuleCast or SphereCast is used depending on the type of collider that has been added.
        /// The result is stored in the m_RaycastHit object.
        /// </summary>
        /// <param name="castCollider">The collider which is performing the cast.</param>
        /// <param name="direction">The direction to perform the cast.</param>
        /// <param name="offset">Any offset to apply to the cast.</param>
        /// <param name="distance">The distance of the cast.</param>
        /// <param name="layers">The layers to perform the cast on.</param>
        /// <param name="raycastHit">The resulting RaycastHit.</param>
        /// <returns>Did the cast hit an object?</returns>
        protected bool SingleCast(Collider castCollider, Vector3 direction, Vector3 offset, float distance, int layers, out RaycastHit raycastHit
#if UNITY_EDITOR
            , bool drawDebugLine = false
#endif
            )
        {
            // Determine if the collider would intersect any objects.
            bool hit;
            if (castCollider is CapsuleCollider) {
                Vector3 capsuleEndCap1, capsuleEndCap2;
                var capsuleCollider = castCollider as CapsuleCollider;
                var colliderTransform = capsuleCollider.transform;
                MathUtils.CapsuleColliderEndCaps(capsuleCollider, colliderTransform.position + offset, colliderTransform.rotation, out capsuleEndCap1, out capsuleEndCap2);
                var radius = capsuleCollider.radius * MathUtils.ColliderScaleMultiplier(capsuleCollider) - c_ColliderSpacing;
                hit = Physics.CapsuleCast(capsuleEndCap1, capsuleEndCap2, radius, direction.normalized, out raycastHit, distance + c_ColliderSpacing, layers, QueryTriggerInteraction.Ignore);
#if UNITY_EDITOR
                if (drawDebugLine) {
                    Debug.DrawLine(capsuleEndCap1, capsuleEndCap2, Color.green);
                    Debug.DrawRay(capsuleEndCap1, direction.normalized * Mathf.Min(distance, 100), Color.blue);
                }
#endif
                return hit;
            } else if (castCollider is SphereCollider) {
                var sphereCollider = castCollider as SphereCollider;
                var radius = sphereCollider.radius * MathUtils.ColliderScaleMultiplier(sphereCollider) - c_ColliderSpacing;
                hit = Physics.SphereCast(sphereCollider.transform.TransformPoint(sphereCollider.center) + offset, radius, direction.normalized,
                                                                out raycastHit, distance + c_ColliderSpacing, layers, QueryTriggerInteraction.Ignore);
#if UNITY_EDITOR
                if (drawDebugLine) {
                    Debug.DrawRay(sphereCollider.transform.TransformPoint(sphereCollider.center) + offset, direction.normalized * Mathf.Min(distance, 100), Color.blue);
                }
#endif
            } else { // BoxCollider.
                var boxCollider = castCollider as BoxCollider;
                var extents = (MathUtils.ColliderScaleMultiplier(boxCollider) - ColliderSpacing) * boxCollider.size / 2;
                hit = Physics.BoxCast(boxCollider.transform.TransformPoint(boxCollider.center) + offset, extents, direction, out raycastHit,
                                                            boxCollider.transform.rotation, distance + ColliderSpacing, layers, QueryTriggerInteraction.Ignore);
#if UNITY_EDITOR
                if (drawDebugLine) {
                    Debug.DrawRay(boxCollider.transform.TransformPoint(boxCollider.center) + offset, direction.normalized * Mathf.Min(distance, 100), Color.blue);
                }
#endif
            }
            return hit;
        }
        
        /// <summary>
        /// Adds a force to the character in the specified number of frames. The force will either be an external or soft force.
        /// </summary>
        /// <param name="force">The force to add.</param>
        /// <param name="frames">The number of frames to add the force to.</param>
        /// <param name="scaleByMass">Should the force be scaled by the character's mass?</param>
        public void AddForce(Vector3 force, int frames = 1, bool scaleByMass = true)
        {
            if (force.sqrMagnitude == 0) {
                return;
            }

            if (scaleByMass) {
                force /= _rigidbody.mass;
            }
            m_ForceUngrounded = m_Grounded && this.InverseTransformDirection(force).y > 0;
            if (frames > 1) {
                AddSoftForce(force, frames);
            } else {
                AddExternalForce(force);
            }
        }
        /// <summary>
        /// Adds a soft force to the character. A soft force is spread out through up to c_MaxSoftForceFrames frames.
        /// </summary>
        /// <param name="force">The force to add.</param>
        /// <param name="frames">The number of frames to add the force to.</param>
        private void AddSoftForce(Vector3 force, float frames)
        {
            frames = Mathf.Clamp(frames, 1, m_MaxSoftForceFrames);
            AddExternalForce(force / frames);
            for (int i = 0; i < (Mathf.RoundToInt(frames) - 1); i++) {
                m_SoftForceFrames[i] += (force / frames);
            }
        }
        /// <summary>
        /// Adds an external force to add.
        /// </summary>
        /// <param name="force">The force to add.</param>
        private void AddExternalForce(Vector3 force)
        {
            m_ExternalForce += force;
        }
        
        /// <summary>
        /// Determine if the character is on the ground.
        /// </summary>
        /// <param name="sendGroundedEvents">Should the events be sent if the grounded status changes?</param>
        private void DetectGroundCollision(bool sendGroundedEvents = true)
        {
            if (!UsingGroundCollisionDetection) {
                return;
            }

            m_GroundedRaycastHit = m_EmptyRaycastHit;
            m_CharacterGroundedCollider = null;

            var grounded = false;
            var localDesiredMovement = MathUtils.InverseTransformDirection(m_DesiredMovement, m_Rotation);
            // The target position should be above the current position to account for slopes.
            var castOffset = m_Radius + m_MaxStepHeight + (m_SlopedGround ? localDesiredMovement.magnitude : 0);
            var targetPosition = m_Position + MathUtils.TransformDirection(
                                new Vector3(localDesiredMovement.x, (localDesiredMovement.y < 0 ? 0 : localDesiredMovement.y) + castOffset, localDesiredMovement.z),
                                m_Rotation);
            var stickyGround = StickingToGround && m_Grounded;
            var hitCount = CombinedCast(targetPosition, m_GravityDirection, 
                                        (stickyGround ? m_StickToGroundDistance : 0) +
                                        m_SkinWidth + castOffset + c_ColliderSpacing);
            if (hitCount > 0) {
                for (int i = 0; i < hitCount; ++i) {
                    var closestRaycastHit = QuickSelect.SmallestK(m_CombinedCastResults, hitCount, i, m_CastHitComparer);
                    if (closestRaycastHit.distance == 0) {
                        if (closestRaycastHit.collider.Raycast(new Ray(targetPosition + m_Up * castOffset, m_GravityDirection), out var hit, Mathf.Infinity) &&
                                    (closestRaycastHit.distance == 0 || MathUtils.InverseTransformPoint(m_Position, m_Rotation, closestRaycastHit.point).y > m_MaxStepHeight)) {
                            var colliderIndex = 0;
                            if (m_ColliderCount > 1) {
                                colliderIndex = m_ColliderIndexMap[closestRaycastHit];
                            }
                            closestRaycastHit = hit;
                            closestRaycastHit.distance = MathUtils.InverseTransformDirection(targetPosition - hit.point, m_Rotation).y;

                            // The raycast result may already exist if there are multiple m_CombinedCastReults with a 0 distance.
                            if (m_ColliderCount > 1 && !m_ColliderIndexMap.ContainsKey(closestRaycastHit)) {
                                m_ColliderIndexMap.Add(closestRaycastHit, colliderIndex);
                            }
                        } else {
                            continue;
                        }
                    }

                    if (this.InverseTransformPoint(closestRaycastHit.point).y > m_Radius + m_MaxStepHeight + (m_SlopedGround ? localDesiredMovement.magnitude : 0)) {
                        continue;
                    }

                    if (closestRaycastHit.rigidbody != null && !closestRaycastHit.rigidbody.isKinematic) {
                        var groundPoint = MathUtils.InverseTransformPoint(m_Position, m_Rotation, closestRaycastHit.point);
                        if (groundPoint.y > m_MaxStepHeight) {
                            continue;
                        }
                    }

                    // Determine which collider caused the intersection.
                    var distance = closestRaycastHit.distance - castOffset;

                    // The character is grounded if:
                    // - The character should not be force ungrounded OR the desired movement is down.
                    // - The character is sticking to the ground or the desired vertical movement is less than the distance to the ground.
                    // - The distance to the ground is less than the skin width. If the desired movement is up then the character can still be grounded if the ground is near.
                    grounded = !m_ForceUngrounded && (stickyGround || localDesiredMovement.y <= Mathf.Abs(distance)) && ((Mathf.Abs(localDesiredMovement.y) + (stickyGround ? (m_StickToGroundDistance + c_ColliderSpacing) : 0) > distance) || distance < m_SkinWidth) && (distance + Mathf.Min(localDesiredMovement.y, 0) <= m_SkinWidth || stickyGround);
                    if (grounded) {
                        m_GravityAccumulation = 0;
                        // if (!m_MovingPlatformOverride && m_GroundedRaycastHit.transform != closestRaycastHit.transform) {
                        //     UpdateMovingPlatformTransform(closestRaycastHit.transform);
                        // }
                        m_GroundedRaycastHit = closestRaycastHit;
                        var activeCollider = m_ColliderCount > 1 ? m_Colliders[m_ColliderIndexMap[closestRaycastHit]] : m_Colliders[m_ColliderIndex];
                        m_CharacterGroundedCollider = activeCollider;

                        if (UsingVerticalCollisionDetection) {
                            var desiredMagnitude = m_DesiredMovement.magnitude;
                            var bouncinessValue = MathUtils.BouncinessValue(activeCollider.material, closestRaycastHit.collider.material);
                            if (bouncinessValue > 0) {
                                var magnitude = desiredMagnitude * bouncinessValue * m_GroundBounceModifier / Time.deltaTime;
                                AddForce(closestRaycastHit.normal * magnitude, 1, false);
                            }
                            // Do not slide on the ground unless the ground friction material is explicitly set.
                            var frictionValue = (Mathf.Clamp01(MathUtils.FrictionValue(m_CharacterGroundedCollider.material, closestRaycastHit.collider.material, true)));
                            if (frictionValue > 0) {
                                var magnitude = desiredMagnitude * frictionValue * m_GroundFrictionModifier;
                                var direction = Vector3.Cross(Vector3.Cross(closestRaycastHit.normal, -m_Up), closestRaycastHit.normal);
                                AddForce(direction * magnitude, 1, false);
                            }

                            PushRigidbody(closestRaycastHit.rigidbody, closestRaycastHit.point, closestRaycastHit.normal, desiredMagnitude);

                            // The character should be snapped to the ground if the ground is sticky or the character would go through the ground (the distance is negative).
                            localDesiredMovement.y -= (stickyGround || distance < 0 || localDesiredMovement.y - distance < 0) ? (distance + Mathf.Min(localDesiredMovement.y, 0) - c_ColliderSpacing) : 0;
                            m_DesiredMovement = MathUtils.TransformDirection(localDesiredMovement, m_Rotation);

                            // Others may be interested in the collision.
                            // if (m_OnCollision != null) {
                            //     m_OnCollision(closestRaycastHit);
                            // }
                        }
                    }
                    break;
                }
            }

            if (UpdateGroundState(grounded, sendGroundedEvents)) {
                if (grounded) {
                    // The vertical external force should be cancelled out.
                    if (!m_ForceUngrounded) {
                        var localExternalForce = this.InverseTransformDirection(m_ExternalForce);
                        if (localExternalForce.y < 0) {
                            localExternalForce.y = 0;
                            m_ExternalForce = this.TransformDirection(localExternalForce);
                        }
                    }
                } else {
                    m_ForceUngrounded = false;
                }
            }
            m_SlopedGround = false;
            UpdateSlopeFactor();
        }
        
        /// <summary>
        /// Updates the grounded state.
        /// </summary>
        /// <param name="grounded">Is the character grounded?</param>
        /// <param name="sendGroundedEvents">Should the events be sent if the grounded status changes?</param>
        /// <returns>True if the grounded state changed.</returns>
        protected virtual bool UpdateGroundState(bool grounded, bool sendGroundedEvents)
        {
            // Update the grounded state. Allows for cleanup when the character hits the ground or moves into the air.
            if (m_Grounded != grounded) {
                m_Grounded = grounded;
                return true;
            }
            return false;
        }
        /// <summary>
        /// Updates the slope factor. This gives the option of slowing the character down while moving up a slope or increasing the speed while moving down.
        /// </summary>
        protected virtual void UpdateSlopeFactor()
        {
            // The character isn't on a slope while in the air or not moving.
            if (!m_AdjustMotorForceOnSlope || !m_Grounded || m_MotorThrottle.sqrMagnitude == 0 || !UsingVerticalCollisionDetection) {
                m_SlopeFactor = 1;
                return;
            }

            // Determine if the slope is uphill or downhill.
            m_SlopeFactor = 1 + (1 - (Vector3.Angle(m_GroundedRaycastHit.normal, m_MotorThrottle.normalized) / 90));

            if (Mathf.Abs(1 - m_SlopeFactor) < 0.01f) { // Moving on flat ground or moving perpendicular to a slope.
                m_SlopeFactor = 1;
            } else if (m_SlopeFactor > 1) { // Downhill.
                m_SlopeFactor = m_MotorSlopeForceDown / m_SlopeFactor;
            } else { // Uphill.
                m_SlopeFactor *= m_MotorSlopeForceUp;
            }
        }

        /// <summary>
        /// Updates the character's position. The DesiredMovement will be set based on the root motion/input values.
        /// </summary>
        private void UpdatePosition()
        {
            var frictionValue = 1f; //摩擦力
            // The collider may be destroyed before the grounded check runs again.
            if (m_Grounded && m_GroundedRaycastHit.collider != null) {
                frictionValue = (1 - Mathf.Clamp01(MathUtils.FrictionValue(m_CharacterGroundedCollider.material, m_GroundedRaycastHit.collider.material, true)));
            }

            // Dampen motor forces.
            m_MotorThrottle /= (1 + m_MotorDamping * Time.timeScale);

            // 如果角色正在向后移动，应用一个乘数。
            var backwardsMultiplier = 1f; //后退乘数
            if (m_InputVector.y < 0)
            {
                backwardsMultiplier *= Mathf.Lerp(1, m_MotorBackwardsMultiplier, Mathf.Abs(m_InputVector.y));
            }

            // As the character changes rotation the same local motor throttle force should be applied. This is most apparent when the character is being aligned to the ground
            // and the local y direction changes.
            var prevLocalMotorThrottle = MathUtils.InverseTransformDirection(m_MotorThrottle, m_PrevMotorRotation) *
                                         m_PreviousAccelerationInfluence;
            var rotation = Quaternion.Slerp(m_PrevMotorRotation, m_Rotation, m_PreviousAccelerationInfluence);
            var acceleration = m_SlopeFactor * backwardsMultiplier * m_MotorAcceleration  * Time.timeScale;
            // Convert input into motor forces. Normalize the input vector to prevent the diagonal from moving faster.
            var normalizedInputVector = m_InputVector.normalized *
                                        Mathf.Max(Mathf.Abs(m_InputVector.x), Mathf.Abs(m_InputVector.y));
            m_MotorThrottle = MathUtils.TransformDirection(new Vector3(
                    prevLocalMotorThrottle.x + normalizedInputVector.x * acceleration.x,
                    prevLocalMotorThrottle.y, prevLocalMotorThrottle.z + normalizedInputVector.y * acceleration.z),
                rotation) * frictionValue;
            m_PrevMotorRotation = m_Rotation;
        }
        /// <summary>
        /// Applies the desired movement to the transform.
        /// </summary>
        protected virtual void ApplyPosition()
        {
            m_Velocity = (m_DesiredMovement) / Time.deltaTime;
            m_Position += m_DesiredMovement;

            // Cancel out the vertical external force if the character changes directions.
            if (LocalDesiredMovement.y > 0) {
                var localExternalForce = transform.InverseTransformDirection(m_ExternalForce);
                if (localExternalForce.y < 0) {
                    localExternalForce.y = 0;
                    m_ExternalForce = transform.TransformDirection(localExternalForce);
                }
            }

            m_LastDesiredMovement = m_DesiredMovement;
            m_DesiredMovement = Vector3.zero;
        }
        /// <summary>
        /// Updates the desired movement value.
        /// </summary>
        protected virtual void UpdateDesiredMovement()
        {
            m_DesiredMovement += (m_MotorThrottle + m_ExternalForce) * Time.deltaTime;

            //计算重力位移
            if (!m_Grounded && UsingGravity) {
                m_GravityAccumulation += m_GravityAmount * Time.timeScale * Time.deltaTime;
                m_DesiredMovement += m_GravityDirection * (m_GravityAccumulation * Time.timeScale);
            }
        }
        /// <summary>
        /// Updates any external forces.
        /// </summary>
        private void UpdateExternalForces()
        {
            // Apply a soft force (forces applied over several frames).
            if (m_SoftForceFrames[0].sqrMagnitude != 0) {
                AddExternalForce(m_SoftForceFrames[0]);
                for (int i = 0; i < m_MaxSoftForceFrames; ++i) {
                    m_SoftForceFrames[i] = (i < m_MaxSoftForceFrames - 1) ? m_SoftForceFrames[i + 1] : Vector3.zero;
                    if (m_SoftForceFrames[i].sqrMagnitude == 0) {
                        break;
                    }
                }
            }

            // Dampen external forces.
            m_ExternalForce /= (1 + m_ExternalForceDamping * Time.timeScale);
        }
        private void ApplyRotation()
        {
            m_Rotation *= m_DesiredRotation;
            m_Rotation.Normalize();

            m_LastDesiredRotation = m_DesiredRotation;
            m_DesiredRotation =  Quaternion.identity;
        }

        
        /// <summary>
        /// Updates the character's rotation. The DesiredRotation will be set based on the root motion/input values.
        /// </summary>
        protected virtual void UpdateRotation()
        {
            // Rotate according to the root motion rotation or target rotation.
            Quaternion rotationDelta, targetRotation;
            var rotation = TargetRotation;
            // m_Rotation = Quaternion.Euler(playerEntity.data.FixedRotation);
            targetRotation = Quaternion.Slerp(rotation, rotation * Quaternion.Euler(m_DeltaRotation), m_MotorRotationSpeed  * Time.timeScale);
            rotationDelta = Quaternion.Inverse(rotation) * targetRotation;

            // Apply the delta rotation.
            m_DesiredRotation *= rotationDelta;
        }
        private bool m_CheckRotationCollision;
        /// <summary>
        /// Performs on overlap cast on the specified collider.
        /// </summary>
        /// <param name="overlapCollider">The collider to check against.</param>
        /// <param name="targetPosition">The position of the collider.</param>
        /// <param name="targetRotation">The rotation of the collider.</param>
        /// <param name="sizeBuffer">The size of the collider that should be modified by when performing the check.</param>
        /// <returns>The number of objects which overlap the collider. These objects will be populated within m_OverlapCastResults.</returns>
        public int OverlapColliders(Collider overlapCollider, Vector3 targetPosition, Quaternion targetRotation, float sizeBuffer = 0)
        {
            int hitCount;
            if (overlapCollider is CapsuleCollider) {
                var capsuleCollider = overlapCollider as CapsuleCollider;
                capsuleCollider.radius += sizeBuffer;
                MathUtils.CapsuleColliderEndCaps(capsuleCollider, targetPosition, targetRotation, out var capsuleEndCap1, out var capsuleEndCap2);
                hitCount = Physics.OverlapCapsuleNonAlloc(capsuleEndCap1, capsuleEndCap2, capsuleCollider.radius * MathUtils.ColliderScaleMultiplier(capsuleCollider),
                                        m_OverlapCastResults, playerMask, QueryTriggerInteraction.Ignore);
                capsuleCollider.radius -= sizeBuffer;
            } else if (overlapCollider is SphereCollider) {
                var sphereCollider = overlapCollider as SphereCollider;
                sphereCollider.radius += sizeBuffer;
                hitCount = Physics.OverlapSphereNonAlloc(targetPosition, sphereCollider.radius * MathUtils.ColliderScaleMultiplier(sphereCollider),
                                        m_OverlapCastResults, playerMask, QueryTriggerInteraction.Ignore);
                sphereCollider.radius -= sizeBuffer;
            } else { // BoxCollider.
                var boxCollider = overlapCollider as BoxCollider;
                boxCollider.size = boxCollider.size + new Vector3(sizeBuffer, sizeBuffer, sizeBuffer);
                var extents = (MathUtils.ColliderScaleMultiplier(boxCollider) - c_ColliderSpacing) * boxCollider.size / 2;
                hitCount = Physics.OverlapBoxNonAlloc(targetPosition, extents, m_OverlapCastResults, targetRotation, playerMask, QueryTriggerInteraction.Ignore);
                boxCollider.size = boxCollider.size - new Vector3(sizeBuffer, sizeBuffer, sizeBuffer);
            }
            return hitCount;
        }
        
        /// <summary>
        /// Returns true if the penetrations have been resolved with the specified collider.
        /// </summary>
        /// <param name="activeCollider">The collider that caused the collision.</param>
        /// <param name="moveDirection">The move direction of the collider.</param>
        /// <param name="offset">The resulting offset of the penetrations.</param>
        /// <returns>True if the penetrations have been resolved with the specified collider.</returns>
        private bool ResolvePenetrations(Collider activeCollider, Vector3 moveDirection, out Vector3 offset)
        {
            offset = Vector3.zero;
            var resolved = false;
            var iterations = 0;

            while (iterations < m_MaxPenetrationChecks && !resolved) {
                resolved = true;
                var hitCount = OverlapColliders(activeCollider, activeCollider.transform.position + moveDirection + offset, activeCollider.transform.rotation);
                if (hitCount > 0) {
                    for (int i = 0; i < hitCount; ++i) {
                        if (Physics.ComputePenetration(activeCollider, activeCollider.transform.position + moveDirection + offset, activeCollider.transform.rotation, m_OverlapCastResults[i],
                                m_OverlapCastResults[i].transform.position, m_OverlapCastResults[i].transform.rotation, out var direction, out var distance)) {
                            offset += direction * (distance + c_ColliderSpacing);
                            resolved = false;
                            break;
                        }
                    }
                }
                iterations++;
            }

            return resolved;
        }
        // protected Vector3 m_FixedPosition;
        // protected Quaternion m_FixedRotation;
        // protected Vector3 m_SmoothPosition;
        // protected Quaternion m_SmoothRotation;
        // private float m_FixedTime;
        
        // public Vector3 PrepareFixedPosition => m_FixedPosition;
        // private void Update()
        // {
        //     if (!Active) return;
        //     if (isOutPlayerControl)
        //     {
        //         return;
        //     }
        //     var interpAmount = (Time.time - m_FixedTime) / Time.fixedDeltaTime;
        //     if (float.IsNaN(interpAmount)) {
        //         interpAmount = 0;
        //     }
        //     MoveCharacters(false, interpAmount);
        // }
        private bool IsGround()
        {
            Vector3 center = _capsuleCollider.transform.position + _capsuleCollider.center;
            center.y -= _capsuleCollider.height * 0.5f;
            center.y += 0.1f;
            return Physics.Raycast(center, Vector3.down, groundCastDistance, groundCastMask);
        }
        
        public void OnRegainBalance()
        {
            Debug.Log("OnRegainBalance");
            _puppetMaster.mode = PuppetMaster.Mode.Disabled;
            LibertyAIUtils.PlayerController.OnRegainBalance();
        }

        public void OnGetUp()
        {
            Debug.Log("OnGetUp");
            //LibertyAIUtils.PlayerController.inputCamera.SetRagdollState(false);
        }

        public void OnLoseBalance()
        {
            Debug.Log("OnLoseBalance");
            // 玩家控制器切換到輸入模式
            LibertyAIUtils.PlayerController.Switch2InputMode();
            LibertyAIUtils.PlayerController.inputController.Active = false;
            //LibertyAIUtils.PlayerController.inputCamera.SetRagdollState(true);
        }
        public void OnLoseBalanceFromPuppet()
        {
            Debug.Log("OnLoseBalanceFromPuppet");
        }
        public void OnLoseBalanceFromGetUp()
        {
            Debug.Log("OnLoseBalanceFromGetUp");
        }
        
        public void OnPuppetCollisionExit()
        {
            Debug.Log("OnPuppetCollisionExit");
            _puppetMaster.mode = PuppetMaster.Mode.Disabled;
        }
        
        /// <summary>
        /// Can the specified collider cause a collision when the character is rotating? The collider can cause a rotation collision when it would rotate on
        /// an axis other than the relative-y axis.
        /// </summary>
        /// <param name="rotationCollider">The collider to check against.</param>
        /// <returns>True if the collider could cause a rotation collision.</returns>
        private bool CanCauseRotationCollision(Collider rotationCollider)
        {
            Vector3 direction;
            if (rotationCollider is CapsuleCollider) {
                Vector3 startEndCap, endEndCap;
                var capsuleCollider = rotationCollider as CapsuleCollider;
                var colliderTransform = rotationCollider.transform;
                // The CapsuleCollider's end caps and the center position must be on the same relative-y axis.
                direction = this.InverseTransformDirection(colliderTransform.TransformPoint(capsuleCollider.center) - m_Position);
                if (Mathf.Abs(direction.x) > 0.0001f || Mathf.Abs(direction.z) > 0.0001f) {
                    return true;
                }
                MathUtils.CapsuleColliderEndCaps(capsuleCollider, colliderTransform.position, colliderTransform.rotation, out startEndCap, out endEndCap);
                direction = this.InverseTransformDirection(startEndCap - endEndCap);
            } else if (rotationCollider is SphereCollider) {
                direction = this.InverseTransformDirection(rotationCollider.transform.TransformPoint((rotationCollider as SphereCollider).center) - m_Position);
            } else {
                return true; // Box Colliders will always cause a rotation collision.
            }
            if (Mathf.Abs(direction.x) > 0.0001f || Mathf.Abs(direction.z) > 0.0001f) {
                return true;
            }
            return false;
        }
        /// <summary>
        /// Transforms the direction from world space to local space. This is similar to Transform.InverseTransformDirection.
        /// </summary>
        /// <param name="characterLocomotion">A reference to the CharacterLocomotion.</param>
        /// <param name="direction">The direction to transform from world space to local space.</param>
        /// <returns>The local space direction.</returns>
        public Vector3 InverseTransformDirection(Vector3 direction)
        {
            return Quaternion.Inverse(m_Rotation) * direction;
        }
        
        /// <summary>
        /// Transforms the direction from local space to world space. This is similar to Transform.TransformDirection.
        /// </summary>
        /// <param name="characterLocomotion">A reference to the CharacterLocomotion.</param>
        /// <param name="direction">The direction to transform from local space to world space.</param>
        /// <returns>The world space direction.</returns>
        public Vector3 TransformDirection(Vector3 direction)
        {
            return playerEntity.viewer.viewerRoot.rotation * direction;
        }
        /// <summary>
        /// Transforms the position from world space to local space. This is similar to Transform.InverseTransformPoint.
        /// </summary>
        /// <param name="characterLocomotion">A reference to the CharacterLocomotion.</param>
        /// <param name="position">The position of the object.</param>
        /// <returns>The local space position.</returns>
        public Vector3 InverseTransformPoint(Vector3 position)
        {
            var diff = position - playerEntity.viewer.viewerRoot.position;
            return Quaternion.Inverse(playerEntity.viewer.viewerRoot.rotation) * diff;
        }
        /// <summary>
        /// Transforms the position from local space to world space. This is similar to Transform.TransformPoint.
        /// </summary>
        /// <param name="characterLocomotion">A reference to the CharacterLocomotion.</param>
        /// <param name="localPosition">The local position of the object.</param>
        /// <returns>The world space position.</returns>
        public Vector3 TransformPoint(Vector3 localPosition)
        {
            return playerEntity.viewer.viewerRoot.position + (playerEntity.viewer.viewerRoot.rotation * localPosition);
        }
        
        /// <summary>
        /// Sets the position of the character.
        /// </summary>
        /// <param name="position">The position to set.</param>
        public virtual void SetPosition(Vector3 position,Vector3 rotation)
        {
            playerEntity.SetPositionAndRotation(position,rotation);
            m_Position = position;
            // m_Rotation = quaternion.Euler(rotation);
            m_MotorThrottle = m_DesiredMovement = m_ExternalForce = Vector3.zero;
            m_GravityAccumulation = 0;
            m_Grounded = false;
            m_SlopedGround = false;
            DetectGroundCollision(false);
        }

        public void SetPlayerEntity(AIEntity aiEntity)
        {
            playerEntity = aiEntity;    
            ResetRotationPosition();
        }
    }
    
    /// <summary>
    /// Allows for equity comparison checks between RaycastHit objects.
    /// </summary>
    public struct RaycastHitEqualityComparer : IEqualityComparer<RaycastHit>
    {
        /// <summary>
        /// Determines if RaycastHit x is equal to RaycastHit y.
        /// </summary>
        /// <param name="x">The first RaycastHit to compare.</param>
        /// <param name="y">The second RaycastHit to compare.</param>
        /// <returns>True if the raycasts are equal.</returns>
        public bool Equals(RaycastHit x, RaycastHit y)
        {
            if (x.distance != y.distance) {
                return false;
            }
            if (x.point != y.point) {
                return false;
            }
            if (x.normal != y.normal) {
                return false;
            }
            if (x.transform != y.transform) {
                return false;
            }
            return true;
        }

        /// <summary>
        /// Returns a hash code for the RaycastHit.
        /// </summary>
        /// <param name="hit">The RaycastHit to get the hash code of.</param>
        /// <returns>The hash code for the RaycastHit.</returns>
        public int GetHashCode(RaycastHit hit)
        {
            // Don't use hit.GetHashCode because that has boxing. This hash function won't always prevent duplicates but it's fine for what it's used for.
            return ((int)(hit.distance * 10000)) ^ ((int)(hit.point.x * 10000)) ^ ((int)(hit.point.y * 10000)) ^ ((int)(hit.point.z * 10000)) ^
                   ((int)(hit.normal.x * 10000)) ^ ((int)(hit.normal.y * 10000)) ^ ((int)(hit.normal.z * 10000));
        }
    }
    
    /// <summary>
    /// Allows for comparison between RaycastHit objects.
    /// </summary>
    public class RaycastHitComparer : IComparer<RaycastHit>
    {
        /// <summary>
        /// Compare RaycastHit x to RaycastHit y. If x has a smaller distance value compared to y then a negative value will be returned.
        /// If the distance values are equal then 0 will be returned, and if y has a smaller distance value compared to x then a positive value will be returned.
        /// </summary>
        /// <param name="x">The first RaycastHit to compare.</param>
        /// <param name="y">The second RaycastHit to compare.</param>
        /// <returns>The resulting difference between RaycastHit x and y.</returns>
        public int Compare(RaycastHit x, RaycastHit y)
        {
            if (x.transform == null) {
                return int.MaxValue;
            }
            if (y.transform == null) {
                return int.MinValue;
            }
            return x.distance.CompareTo(y.distance);
        }
    }
}